#!/usr/bin/env python
#encoding=utf-8

import sys, select, os, moveit_commander
if os.name == 'nt':
    import msvcrt
else:
    import tty, termios
import rospy
import actionlib
from control_msgs.msg import (FollowJointTrajectoryAction,
                              FollowJointTrajectoryGoal,
                              GripperCommandAction,
                              GripperCommandGoal)
from geometry_msgs.msg import Twist
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
import time
from tf.transformations import euler_from_quaternion,quaternion_from_euler
from gazebo_msgs.srv import *
from std_srvs.srv import Empty
from geometry_msgs.msg import *
import numpy as np

FETCH_MAX_LIN_VEL = 1
FETCH_MAX_ANG_VEL = 2.84
LIN_VEL_STEP_SIZE = 0.01
ANG_VEL_STEP_SIZE = 0.1

def checkLinearLimitVelocity(vel):
    vel = constrain(vel, -FETCH_MAX_LIN_VEL, FETCH_MAX_LIN_VEL)
    return vel

def checkAngularLimitVelocity(vel):
    vel = constrain(vel, -FETCH_MAX_ANG_VEL, FETCH_MAX_ANG_VEL)
    return vel

def constrain(input, low, high):
    if input < low:
        input = low
    elif input > high:
        input = high
    else:
        input = input

    return input

def makeSimpleProfile(output, input, slop):
    if input > output:
        output = min( input, output + slop )
    elif input < output:
        output = max( input, output - slop )
    else:
        output = input

    return output

def main():
    # fetch转身
    status = 0
    target_linear_vel   = 0.0
    target_angular_vel  = 0.0
    control_linear_vel  = 0.0
    control_angular_vel = 0.0
    
    if os.name != 'nt':
        settings = termios.tcgetattr(sys.stdin)
    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

    for i in range(21):
        target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE)
        # target_angular_vel = 0.5
        twist = Twist()

        control_linear_vel = makeSimpleProfile(control_linear_vel, target_linear_vel, (LIN_VEL_STEP_SIZE/2.0))
        twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0

        control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
        twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel

        pub.publish(twist)
        # print(i, control_angular_vel)
        time.sleep(0.4)

    # 设置参数
    # 手臂
    arm_joint_names = ["torso_lift_joint","shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
                "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
    arm_joint_positions_put = [0,0.0,-0.90,0.0,1.05,0.0,1.42,1.57]
    #夹取器
    gripper_joint_names = ["l_gripper_finger_joint","r_gripper_finger_joint"]
    gripper_positions = [0.01,0.03,0.05,0.06,0.1]
    # 连接服务器
    arm_client = actionlib.SimpleActionClient("arm_with_torso_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
    arm_client.wait_for_server()
    gripper_client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction)
    gripper_client.wait_for_server()
    # 传递参数
    # 身体
    trajectory = JointTrajectory()
    trajectory.joint_names = arm_joint_names
    trajectory.points.append(JointTrajectoryPoint())
    trajectory.points[0].positions = arm_joint_positions_put
    trajectory.points[0].velocities =  [0.0] * len(arm_joint_positions_put)
    trajectory.points[0].accelerations = [0.0] * len(arm_joint_positions_put)
    trajectory.points[0].time_from_start = rospy.Duration(3.0)
    arm_goal = FollowJointTrajectoryGoal()
    arm_goal.trajectory = trajectory
    arm_goal.goal_time_tolerance = rospy.Duration(5.0)
    # 夹取器
    gripper_goal = GripperCommandGoal()
    gripper_goal.command.max_effort = 30.0
    gripper_goal.command.position = gripper_positions[4]
    # 降下身体
    arm_client.send_goal(arm_goal)
    arm_client.wait_for_result(rospy.Duration(0.0))

    # 放入小车
    # 获取小车和fetch状态
    while True:
        rospy.wait_for_service('/gazebo/get_model_state')
        get_state_service = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
        model = GetModelStateRequest()
        model.model_name = 'racecar'
        objstate = get_state_service(model)
        state_racecar = [objstate.pose.position.x,objstate.pose.position.y,objstate.pose.position.z, euler_from_quaternion([objstate.pose.orientation.x,objstate.pose.orientation.y,objstate.pose.orientation.z,objstate.pose.orientation.w])[2]]
        print("racecar: ", state_racecar)
        model.model_name = 'fetch'
        objstate = get_state_service(model)
        state_fetch = [objstate.pose.position.x,objstate.pose.position.y,objstate.pose.position.z, euler_from_quaternion([objstate.pose.orientation.x,objstate.pose.orientation.y,objstate.pose.orientation.z,objstate.pose.orientation.w])[2]]
        print("fetch: ", state_fetch)
        # 寻找放下位置
        moveit_commander.roscpp_initialize(sys.argv)
        a = -0.10
        rospy.wait_for_service("/clear_octomap")
        clear_octomap = rospy.ServiceProxy("/clear_octomap",Empty)
        arm = moveit_commander.MoveGroupCommander("arm")
        arm.allow_replanning(True)
        p = Pose()
        x = state_racecar[0] + a*np.cos(state_racecar[3]) - state_fetch[0]
        y = state_racecar[1] + a*np.sin(state_racecar[3]) - state_fetch[1]
        p.position.x = x*np.cos(state_fetch[3]) + y*np.sin(state_fetch[3])
        p.position.y = -x*np.sin(state_fetch[3]) + y*np.cos(state_fetch[3])
        p.position.z = 0.5
        p.orientation = Quaternion(*quaternion_from_euler(1.57, 1.57, state_racecar[3] - state_fetch[3]))
        print(p)
        arm.set_pose_target(p) # <5>
        print("-"*30)
        if arm.go(True):
            break
        clear_octomap()
    # 松开夹子
    gripper_client.send_goal(gripper_goal)
    gripper_client.wait_for_result(rospy.Duration(0.0))

if __name__ == "__main__":
    rospy.init_node("put_item")
    main()




